//
// Created by Wirano on 2020/11/9.
//

#include "Ano_MagProcess.h"
#include "Drv_led.h"

static int16_t max_t[VEC_XYZ];
static int16_t min_t[VEC_XYZ];


_mag_cal_st mag;

static void Mag_Cal_Reset(uint8_t mode)
{
    if(mode == 2)
    {
        for(uint8_t i = 0;i<2;i++)
        {
            max_t[i] = -30000;
            min_t[i] = 30000;

        }
    }
    else if(mode == 1)
    {
        max_t[Z] = -30000;
        min_t[Z] = 30000;
    }
    else
    {
        for(uint8_t i = 0;i<3;i++)
        {
            max_t[i] = -30000;
            min_t[i] = 30000;

        }
    }
}

static void Mag_Cal_XY(int16_t mag_in[])
{
    for(uint8_t i = 0;i<2;i++)
    {
        max_t[i] = _MAX(max_t[i],mag_in[i]);
        min_t[i] = _MIN(min_t[i],mag_in[i]);
    }



}

static void Mag_Cal_Z(int16_t mag_in[])
{
    max_t[Z] = _MAX(max_t[Z],mag_in[Z]);
    min_t[Z] = _MIN(min_t[Z],mag_in[Z]);
}

static uint8_t mag_cal_step;

void Mag_Data_Deal_Task(uint8_t dT_ms,int16_t mag_in[],float z_vec_z,float gyro_deg_x,float gyro_deg_z)
{
    static uint16_t cali_cnt;
    static float mag_cal_angle[2];
    float t_length;

    for(uint8_t i = 0;i<3;i++)
    {
        save.mag_gain[i] = LIMIT(save.mag_gain[i],0.05f,100);
        mag.val[i] = (mag_in[i] - save.mag_offset[i]) *save.mag_gain[i];
    }

///////////////////cali//////////////////////////////////////////////////////	
    if(mag.mag_CALIBRATE!= 0 && flag.unlock_sta == 0)
    {
        switch(mag_cal_step)
        {
            case 0://第一步，水平旋转

                Mag_Cal_XY(mag_in);

                if(z_vec_z<0.985f)//+-10deg	
                {
                    LED_STA.calMag = 100;
                    mag_cal_step = 1;
                    mag_cal_angle[0] = 0;
                }
                else
                {
                    LED_STA.calMag = 1;
                    mag_cal_angle[0] += dT_ms *1e-3f *(gyro_deg_z); //角度积分，旋转360度
                    if(ABS(mag_cal_angle[0])>360)
                    {
                        mag_cal_angle[0] = 0;
                        mag_cal_step = 2;
                    }
                }
                break;

            case 1://error

                Mag_Cal_Reset(2);
                mag_cal_angle[0] = 0;
                mag_cal_step = 0;
                break;

            case 2://第二步，竖直旋转，机头朝下
                LED_STA.calMag = 2;
                if(z_vec_z<0.1f)//5.7deg
                {
                    mag_cal_step = 3;
                }
                break;

            case 3:
                mag.mag_CALIBRATE = 2;

                Mag_Cal_Z(mag_in);

                if(z_vec_z>0.17f)//10deg
                {
                    LED_STA.calMag = 2;
                    mag_cal_step = 4;
                    mag_cal_angle[1] = 0;
                }
                else
                {
                    LED_STA.calMag = 3;
                    mag_cal_angle[1] += dT_ms *1e-3f *(gyro_deg_x);	//角度积分，旋转360度
                    if(ABS(mag_cal_angle[1])>360)
                    {
                        mag_cal_angle[1] = 0;
                        mag_cal_step = 5;
                    }
                }
                break;

            case 4://error_2，重新开始竖直旋转
                Mag_Cal_Reset(1);
                mag_cal_angle[1] = 0;
                mag_cal_step = 2;
                break;

            case 5:
                for(uint8_t i = 0;i<3;i++)
                {
                    save.mag_offset[i] = 0.5f *(max_t[i] + min_t[i]);		//中值校准
                    save.mag_gain[i] = safe_div(200.0f ,(0.5f *(max_t[i] - min_t[i])),0);		//幅值校准
                }

                Mag_Cal_Reset(3);
                mag_cal_angle[0] = mag_cal_angle[1] = 0;
                mag_cal_step = 0;
                mag.mag_CALIBRATE = 0;
                LED_STA.calMag = 0;

                data_save();//保存数据
                break;

            default:break;
        }


        if(mag_cal_step == 0 || mag_cal_step == 3)
        {
            //长时间出错，退出校准逻辑
            if(cali_cnt<15000)
            {
                cali_cnt+= dT_ms;

            }
            else////校准错误
            {
                LED_STA.errOneTime = 1;
                cali_cnt = 0;
                LED_STA.calMag = 0;
                mag.mag_CALIBRATE = 0;

            }
        }
        else
        {
            cali_cnt = 0;
        }
    }
    else
    {
        //if(LED_state ==4 || LED_state ==5) LED_state = 0;

        mag_cal_step = 0;

//////////////////////////////////////////////	
        t_length = my_3_norm(mag.val[X],mag.val[Y],mag.val[Z]);

        if(t_length<150||t_length>350)
        {
            //state[3] |= (1<<3);//罗盘严重干扰
            //LED_state = 6;
        }
        else
        {
            //state[3] &= ~(1<<3);//罗盘无严重干扰

//			if(LED_state == 6)
//			{
//				LED_state = 0;
//			}
        }
///////////////////////////////////////////////	
    }
}

